% 恒速优化
function state = speedplan_cruise(lineStruct, trainStruct, qsStruct)
% 待求解问题
set_time = qsStruct.set_time; %剩余运行时间 s
pos_start = qsStruct.pos_start * 1000; %起始位置 m
pos_end = qsStruct.pos_end * 1000; %末位置 m
v_start = qsStruct.v_start; %初始速度 km/h
v_end = qsStruct.v_end; %末速度 km/h
step_x = qsStruct.step_x; %求解步长

t_err = 10; % 恒速优化时间误差 s
ori_speedlimit = lineStruct.speedlimit;
rows_speedlimit = size(ori_speedlimit,1);

%先求一次最大能力
state_max = speedPlanMP(lineStruct, trainStruct, qsStruct);
%求最短运行时间
t_min = state_max(end,3);
fprintf('最大能力时间为：%f s\n',t_min);
if set_time < t_min
    fprintf('预设时间小于最短运行时间\n');
    state = state_max;
    return;
end
%求满足最低限速时的时间
[min_speedlimit, idx] = min(ori_speedlimit(:,3));
temp_speedlimit = [ori_speedlimit(:,1:2) ones(rows_speedlimit,1)*min_speedlimit];
line_struct = lineStruct;
line_struct.speedlimit = temp_speedlimit;
state1 = speedPlanMP(line_struct, trainStruct, qsStruct);
t1 = state1(end,3);
% %判断如果维持当前恒速时分约束是否无法满足
% if v_start ~= 0 && v_start < inquire_linedata(pos_start,ori_speedlimit) % 初始速度不为0
%     temp_speedlimit = [ori_speedlimit(:,1:2) ones(rows_speedlimit,1)*(v_start+1)];
%     line_struct.speedlimit = temp_speedlimit;
%     state1 = speedPlanMP(line_struct, trainStruct, qsStruct);
%     t2 = state1(end,3);
% end
%判断往上找还是往下找
if set_time > t1  %预期时间比最低限速等级下恒速还长，往下找
    vlimit_up = min_speedlimit;
    t_up = t1;
    t_down = t_up;
    if qsStruct.v_start == 0
        vlimit_down = vlimit_up;
    else
        vlimit_down = (qsStruct.v_start+1)*2; %为了统一，后面有个除以2
    end
    %找下界
    while t_down < set_time % 下界时间应该比预设时间长
        vlimit_down = 1/2 * vlimit_down;
        temp_speedlimit = [ori_speedlimit(:,1:2) ones(rows_speedlimit,1)*vlimit_down];
        line_struct.speedlimit = temp_speedlimit;
        state1 = speedPlanMP(line_struct, trainStruct, qsStruct);
        t_down = state1(end,3);
    end
    %二分搜索恒速区间
    t_mid = t_up;
    while abs(t_mid-set_time) > t_err
        vlimit_mid = (vlimit_up+vlimit_down)/2;
        temp_speedlimit = [ori_speedlimit(:,1:2) ones(rows_speedlimit,1)*vlimit_mid];
        line_struct.speedlimit = temp_speedlimit;
        state1 = speedPlanMP(line_struct, trainStruct, qsStruct);
        t_mid = state1(end,3);
        if t_mid < set_time %快了，降高限速
            vlimit_up = vlimit_mid;
        else
            vlimit_down = vlimit_mid;
        end
    end
elseif set_time < t1 %预期时间比最低限速等级下恒速还短，说明恒速在最大能力和最低限速等级之间，往上找
    temp_speedlimit = ori_speedlimit;
    vlimit_down = min_speedlimit;
    t_down = t1;
    vlimit_up = max(temp_speedlimit(:,3));
    t_up = t_min;
    t_mid = t_up;

    while abs(t_mid-set_time) > t_err
        vlimit_mid = (vlimit_up+vlimit_down)/2;
        for i = 1:rows_speedlimit
            if ori_speedlimit(i,3) > vlimit_mid
                temp_speedlimit(i,3) = vlimit_mid;
            else
                temp_speedlimit(i,3) = ori_speedlimit(i,3);
            end
        end
        line_struct.speedlimit = temp_speedlimit;
        state1 = speedPlanMP(line_struct, trainStruct, qsStruct);
        t_mid = state1(end,3);
        if t_mid < set_time %快了，降高限速
            vlimit_up = vlimit_mid;
        else
            vlimit_down = vlimit_mid;
        end
    end
end

state = state1;